#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static cv::RotatedRect _fit_ellipse(const std::vector<cv::Point>& points) {
    int n = points.size();
    if (n < 5) {
        CV_Error(cv::Error::StsBadSize, "at least 5 points to fit the ellipse");
    }
    const double eps = 1e-8;
    
    cv::Point2f pt_mean(0, 0);
    
    cv::Mat M(n, 5, CV_64FC1);
    cv::Mat N(n, 1, CV_64FC1);
    cv::Mat X = cv::Mat::zeros(5, 1, CV_64FC1);
    
    for (int i = 0; i < n; i++) {
        cv::Point2f p = cv::Point2f((float)points[i].x, (float)points[i].y);
        pt_mean += p;
    }
    pt_mean.x /= n;
    pt_mean.y /= n;
    
    for (int i = 0; i < n; i++) {
        cv::Point2f p = cv::Point2f((float)points[i].x, (float)points[i].y);
        p -= pt_mean;
        
        N.ptr<double>(i)[0] = 1;
        M.ptr<double>(i)[0] = (double)p.x * p.x;
        M.ptr<double>(i)[1] = (double)p.x * p.y;
        M.ptr<double>(i)[2] = (double)p.y * p.y;
        M.ptr<double>(i)[3] = p.x;
        M.ptr<double>(i)[4] = p.y;
    }
    cv::solve(M, N, X, cv::DECOMP_SVD);
    double A = X.ptr<double>(0)[0];
    double B = X.ptr<double>(1)[0];
    double C = X.ptr<double>(2)[0];
    double D = X.ptr<double>(3)[0];
    double E = X.ptr<double>(4)[0];
    
    cv::Mat MM = cv::Mat(2, 2, CV_64FC1);
    cv::Mat NN = cv::Mat(2, 1, CV_64FC1);
    cv::Mat XX = cv::Mat(2, 1, CV_64FC1);
    MM.ptr<double>(0)[0] = -2 * A;
    MM.ptr<double>(0)[1] = MM.ptr<double>(1)[0] = -B;
    MM.ptr<double>(1)[1] = -2 * C;
    NN.ptr<double>(0)[0] = D;
    NN.ptr<double>(1)[0] = E;
    cv::solve(MM, NN, XX, cv::DECOMP_SVD);
    double x0 = XX.ptr<double>(0)[0];
    double y0 = XX.ptr<double>(1)[0];
    
    M = cv::Mat(n, 3, CV_64FC1);
    N = cv::Mat(n, 1, CV_64FC1);
    X = cv::Mat(3, 1, CV_64FC1);
    
    for (int i = 0; i < n; i++) {
        cv::Point2f p = cv::Point2f((float)points[i].x, (float)points[i].y);
        p -= pt_mean;
        N.ptr<double>(i)[0] = 1;
        M.ptr<double>(i)[0] = (p.x - x0) * (p.x - x0);
        M.ptr<double>(i)[1] = (p.x - x0) * (p.y - y0);
        M.ptr<double>(i)[2] = (p.y - y0) * (p.y - y0);
    }
    cv::solve(M, N, X, cv::DECOMP_SVD);
    A = X.ptr<double>(0)[0];
    B = X.ptr<double>(1)[0];
    C = X.ptr<double>(2)[0];
    double theta = -0.5 * std::atan2(B, C-A);
    double t;
    if (std::fabs(B) <= eps) {
        t = A-C;
    } else {
        t = B / std::sin(-2.0*theta);
    }
    double a = std::fabs(A + C-t);
    if (a > eps) {
        a = std::sqrt(2.0 / a);
    }
    double b = std::fabs(A +C + t);
    if (b > eps) {
        b = std::sqrt(2.0 / b);
    }
    
    cv::RotatedRect box;
    box.center.x = (float)x0 + pt_mean.x;
    box.center.y = (float)y0 + pt_mean.y;
    box.size.width = (float)(a*2);
    box.size.height = (float)(b*2);
    if (box.size.width > box.size.height) {
        cv::swap(box.size.width, box.size.height);
        box.angle = (float)(90 + theta * 180 / CV_PI);
    } else {
        box.angle = (float)(theta * 180 / CV_PI);
    }
    if (box.angle < -180) {
        box.angle += 360;
    }
    if (box.angle > 360) {
        box.angle -= 360;
    }
    return box;
}

static cv::RotatedRect _fit_ellipse_direct(const std::vector<cv::Point>& points) {
    int n = points.size();
    if (n < 5) {
        CV_Error(cv::Error::StsBadSize, "at least 5 points to fit the ellipse");
    }
    
    cv::Point2f pt_mean(0, 0);
    for (int i = 0; i < n; i++) {
        cv::Point2f p = cv::Point2f((float)points[i].x, (float)points[i].y);
        pt_mean += p;
    }
    pt_mean.x /= n;
    pt_mean.y /= n;
    
    cv::Mat D(n, 6, CV_64FC1);
    for (int i = 0; i < n; i++) {
        cv::Point2f p = cv::Point2f((float)points[i].x, (float)points[i].y);
        p -= pt_mean;
        
        D.ptr<double>(i)[0] = (double)p.x * p.x;
        D.ptr<double>(i)[1] = (double)p.x * p.y;
        D.ptr<double>(i)[2] = (double)p.y * p.y;
        D.ptr<double>(i)[3] = (double)p.x;
        D.ptr<double>(i)[4] = (double)p.y;
        D.ptr<double>(i)[5] = 1.0;
    }
    cv::Mat S(6, 6, CV_64FC1);
    cv::mulTransposed(D, S, true, cv::noArray(), 1.0, -1);
    
    cv::Mat S1 = S(cv::Rect(0, 0, 3, 3));
    cv::Mat S2 = S(cv::Rect(3, 0, 3, 3));
    cv::Mat S3 = S(cv::Rect(3, 3, 3, 3));
    cv::Mat S2T;
    cv::transpose(S2, S2T);
    
    //double DetS3 = cv::determinant(S3);
    cv::Mat InvS3(3, 3, CV_64FC1);
    cv::invert(S3, InvS3);
    cv::Mat MinusInvS3S2T(3, 3, CV_64FC1);
    MinusInvS3S2T = InvS3 * S2T;
    MinusInvS3S2T *= -1.;
    
    cv::Mat InvC1 = cv::Mat::zeros(3, 3, CV_64FC1);
    InvC1.ptr<double>(0)[2] = 0.5;
    InvC1.ptr<double>(1)[1] = -1.;
    InvC1.ptr<double>(2)[0] = 0.5;
    
    cv::Mat InvC1S1, MinusInvC1S2InvS3S2T;
    InvC1S1 = InvC1 * S1;
    MinusInvC1S2InvS3S2T = InvC1 * S2 * MinusInvS3S2T;
    
    cv::Mat M(3, 3, CV_64FC1);
    cv::add(InvC1S1, MinusInvC1S2InvS3S2T, M);
    
    cv::RotatedRect box;
    if (std::fabs(cv::determinant(M)) > 1.0e-10) {
        cv::Mat lambda, a1;
        cv::eigenNonSymmetric(M, lambda, a1);
        
        double A1 = a1.ptr<double>(0)[0], B1 = a1.ptr<double>(0)[1], C1 = a1.ptr<double>(0)[2];
        double A2 = a1.ptr<double>(1)[0], B2 = a1.ptr<double>(1)[1], C2 = a1.ptr<double>(1)[2];
        double A3 = a1.ptr<double>(2)[0], B3 = a1.ptr<double>(2)[1], C3 = a1.ptr<double>(2)[2];
        double constraint[3];
        constraint[0] = 4.0 * A1 * C1 - B1 * B1;
        constraint[1] = 4.0 * A2 * C2 - B2 * B2;
        constraint[2] = 4.0 * A3 * C3 - B3 * B3;
        int idx;
        if (constraint[0] < constraint[1]) {
            idx = (constraint[1] < constraint[2] ? 2 : 1);
        } else {
            idx = (constraint[0] < constraint[2] ? 2 : 0);
        }
        double A = a1.ptr<double>(idx)[0], B = a1.ptr<double>(idx)[1], C = a1.ptr<double>(idx)[2];
        double norm = std::sqrt(A*A + B*B + C*C);
        if ((A < 0.0 ? -1 : 1) * (B < 0.0 ? -1 : 1) * (C < 0.0 ? -1 : 1) <= 0.0) {
            norm *= -1.;
        }
        A /= norm;
        B /= norm;
        C /= norm;
        
        double D = MinusInvS3S2T.ptr<double>(0)[0] * A + MinusInvS3S2T.ptr<double>(0)[1] * B + MinusInvS3S2T.ptr<double>(0)[2] * C;
        double E = MinusInvS3S2T.ptr<double>(1)[0] * A + MinusInvS3S2T.ptr<double>(1)[1] * B + MinusInvS3S2T.ptr<double>(1)[2] * C;
        double F = MinusInvS3S2T.ptr<double>(2)[0] * A + MinusInvS3S2T.ptr<double>(2)[1] * B + MinusInvS3S2T.ptr<double>(2)[2] * C;
        
        double t1 = A*E*E + C*D*D + B*B*F - B*D*E - 4.*A*C*F;
        double t2 = B*B - 4.*A*C;
        double t3 = std::sqrt(B*B + (A-C)*(A-C));
        double t4 = A + C;
        double t5 = 2*C*D - B*E;
        double t6 = 2*A*E - B*D;
        
        double x0, y0, a, b, theta;
        x0 = t5/t2 + pt_mean.x;
        y0 = t6/t2 + pt_mean.y;
        a = std::sqrt(2.0) * std::sqrt(t1/(t2*(t3-t4)));
        b = std::sqrt(2.0) * std::sqrt(-t1/(t2*(t3+t4)));
        if (B == 0) {
            if (A < C) {
                theta = 0;
            } else {
                theta = CV_PI / 2.;
            }
        } else {
            theta = CV_PI / 2. + 0.5 * std::atan2(B, A-C);
        }
        box.center.x = (float)x0;
        box.center.y = (float)y0;
        box.size.width = (float)(2.0 * a);
        box.size.height = (float)(2.0 * b);
        if (box.size.width > box.size.height) {
            cv::swap(box.size.width, box.size.height);
            box.angle = (float)fmod(90 + theta * 180 / CV_PI, 180.0);
        } else {
            box.angle = (float)fmod(theta*180/CV_PI, 180.0);
        }
    } else {
        box = _fit_ellipse(points);
    }
    return box;
}

static cv::RotatedRect _fit_ellipse_ams(const std::vector<cv::Point>& points) {
    int n = points.size();
    if (n < 5) {
        CV_Error(cv::Error::StsBadSize, "at least 5 points to fit the ellipse");
    }
    
    cv::Point2f pt_mean(0, 0);
    for (int i = 0; i < n; i++) {
        cv::Point2f p = cv::Point2f((float)points[i].x, (float)points[i].y);
        pt_mean += p;
    }
    pt_mean.x /= n;
    pt_mean.y /= n;
    
    cv::Mat D(n, 6, CV_64FC1);
    cv::Mat Dx(n, 6, CV_64FC1);
    cv::Mat Dy(n, 6, CV_64FC1);
    for (int i = 0; i < n; i++) {
        cv::Point2f p = cv::Point2f((float)points[i].x, (float)points[i].y);
        p -= pt_mean;
        
        D.ptr<double>(i)[0] = (double)p.x * p.x;
        D.ptr<double>(i)[1] = (double)p.x * p.y;
        D.ptr<double>(i)[2] = (double)p.y * p.y;
        D.ptr<double>(i)[3] = (double)p.x;
        D.ptr<double>(i)[4] = (double)p.y;
        D.ptr<double>(i)[5] = 1.0;
        
        Dx.ptr<double>(i)[0] = (double)p.x * 2.;
        Dx.ptr<double>(i)[1] = (double)p.y;
        Dx.ptr<double>(i)[2] = 0;
        Dx.ptr<double>(i)[3] = 1.;
        Dx.ptr<double>(i)[4] = 0;
        Dx.ptr<double>(i)[5] = 0;
        
        Dy.ptr<double>(i)[0] = 0;
        Dy.ptr<double>(i)[1] = (double)p.x;
        Dy.ptr<double>(i)[2] = (double)p.y * 2.;
        Dy.ptr<double>(i)[3] = 0;
        Dy.ptr<double>(i)[4] = 1.;
        Dy.ptr<double>(i)[5] = 0;
    }
    
    cv::Mat S(6, 6, CV_64FC1);
    cv::Mat DxTDx(6, 6, CV_64FC1);
    cv::Mat DyTDy(6, 6, CV_64FC1);
    cv::mulTransposed(D, S, true, cv::noArray(), 1.0, -1);
    cv::mulTransposed(Dx, DxTDx, true, cv::noArray(), 1.0, -1);
    cv::mulTransposed(Dy, DyTDy, true, cv::noArray(), 1.0, -1);
    S *= (1.0/n);
    
    cv::Mat DxTDx_DyTDy(6, 6, CV_64FC1);
    cv::add(DxTDx, DyTDy, DxTDx_DyTDy);
    DxTDx_DyTDy *= (1.0/n);
    cv::Mat InvDxTDx_DyTDy(5, 5, CV_64FC1);
    cv::invert(DxTDx_DyTDy(cv::Rect(0, 0, 5, 5)), InvDxTDx_DyTDy);
    
    std::vector<double> S_Row5(5), S_Col5(5);
    S_Row5[0] = S.ptr<double>(5)[0];
    S_Row5[1] = S.ptr<double>(5)[1];
    S_Row5[2] = S.ptr<double>(5)[2];
    S_Row5[3] = S.ptr<double>(5)[3];
    S_Row5[4] = S.ptr<double>(5)[4];
    S_Col5[0] = S.ptr<double>(0)[5];
    S_Col5[1] = S.ptr<double>(1)[5];
    S_Col5[2] = S.ptr<double>(2)[5];
    S_Col5[3] = S.ptr<double>(3)[5];
    S_Col5[4] = S.ptr<double>(4)[5];
    for (int i = 0; i < 5; i++) {
        S.ptr<double>(i)[0] = S.ptr<double>(i)[0] - S_Row5[0] * S_Col5[i];
        S.ptr<double>(i)[1] = S.ptr<double>(i)[1] - S_Row5[1] * S_Col5[i];
        S.ptr<double>(i)[2] = S.ptr<double>(i)[2] - S_Row5[2] * S_Col5[i];
        S.ptr<double>(i)[3] = S.ptr<double>(i)[3] - S_Row5[3] * S_Col5[i];
        S.ptr<double>(i)[4] = S.ptr<double>(i)[4] - S_Row5[4] * S_Col5[i];
    }
    cv::Mat M = InvDxTDx_DyTDy * S(cv::Rect(0, 0, 5, 5));
    
    cv::RotatedRect box;
    if (std::fabs(cv::determinant(M)) > 1.0e-10) {
        cv::Mat lambda, a1;
        cv::eigenNonSymmetric(M, lambda, a1);
        
        int minEigenValueIdx = 0;
        double normEigenVector, normEigenValue, normMinEigenVector, normMinEigenValue;
        cv::Mat aRow = a1(cv::Rect(0, minEigenValueIdx, a1.cols, 1));
        cv::Mat squareARow;
        cv::multiply(aRow, aRow, squareARow);
        normMinEigenVector = std::sqrt(cv::sum(squareARow)[0]);
        normMinEigenValue = lambda.ptr<double>(minEigenValueIdx)[0] * normMinEigenVector;
        for (int i = 1; i < 5; i++) {
            aRow = a1(cv::Rect(0, i, a1.cols, 1));
            cv::multiply(aRow, aRow, squareARow);
            normEigenVector = std::sqrt(cv::sum(squareARow)[0]);
            normEigenValue = lambda.ptr<double>(i)[0] * normEigenVector;
            if (normEigenValue < normMinEigenValue) {
                minEigenValueIdx = i;
                normMinEigenValue = normEigenValue;
                normMinEigenVector = normEigenVector;
            }
        }
        
        double A, B, C, D, E, F;
        A = a1.ptr<double>(minEigenValueIdx)[0] / normMinEigenVector;
        B = a1.ptr<double>(minEigenValueIdx)[1] / normMinEigenVector;
        C = a1.ptr<double>(minEigenValueIdx)[2] / normMinEigenVector;
        D = a1.ptr<double>(minEigenValueIdx)[3] / normMinEigenVector;
        E = a1.ptr<double>(minEigenValueIdx)[4] / normMinEigenVector;
        F = -A*S_Col5[0] - B*S_Col5[1] - C*S_Col5[2] - D*S_Col5[3] - E*S_Col5[4];
        
        bool is_ellipse = (A > 0 && 4.*A*C > B*B && F < (A*E*E + C*D*D - B*D*E) / (4.*A*C-B*B)) ||
                        (A < 0 && 4.*A*C > B*B && F > (A*E*E + C*D*D - B*D*E) / (4.*A*C-B*B));
        if (is_ellipse) {
            double t1 = A*E*E + C*D*D + B*B*F - B*D*E - 4.*A*C*F;
            double t2 = B*B - 4.*A*C;
            double t3 = std::sqrt(B*B + (A-C)*(A-C));
            double t4 = A+C;
            double t5 = 2*C*D-B*E;
            double t6 = 2*A*E-B*D;
            
            double x0, y0, a, b, theta;
            x0 = t5/t2 + pt_mean.x;
            y0 = t6/t2 + pt_mean.y;
            a = std::sqrt(2.0) * std::sqrt(t1/(t2*(t3-t4)));
            b = std::sqrt(2.0)*std::sqrt(-t1/(t2*(t3+t4)));
            if (B == 0) {
                if (A<C) {
                    theta = 0;
                } else {
                    theta = CV_PI / 2.;
                }
            } else {
                theta = CV_PI / 2. + 0.5 * std::atan2(B, A-C);
            }
            box.center.x = (float)x0;
            box.center.y = (float)y0;
            box.size.width = (float)(2.0*a);
            box.size.height = (float)(2.0*b);
            if (box.size.width > box.size.height) {
                cv::swap(box.size.width, box.size.height);
                box.angle = (float)fmod(90+theta*180/CV_PI, 180.0);
            } else {
                box.angle = (float)fmod(theta*180 / CV_PI, 180.0);
            }
        } else {
            box = _fit_ellipse_direct(points);
        }
    } else {
        box = _fit_ellipse(points);
    }
    return box;
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/templ.png", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    cv::copyMakeBorder(src, src, 100, 100, 100, 100, cv::BORDER_CONSTANT, cv::Scalar(255, 255, 255));
    cv::Mat gray, binary;
    cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
    cv::threshold(gray, binary, 128, 255, cv::THRESH_BINARY_INV);
    
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(binary, contours, hierarchy, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
    if (contours.size() < 1) {
        return EXIT_FAILURE;
    }
    
    cv::RotatedRect rect1, rect2, rect3;
    rect1 = cv::fitEllipseDirect(contours[0]);
    rect2 = cv::fitEllipseAMS(contours[0]);
    rect3 = cv::fitEllipse(contours[0]);
    cv::Mat src1 = src.clone(), src2 = src.clone(), src3 = src.clone();
    cv::ellipse(src1, rect1, cv::Scalar(0, 0, 255), 3);
    cv::ellipse(src2, rect2, cv::Scalar(0, 0, 255), 3);
    cv::ellipse(src3, rect3, cv::Scalar(0, 0, 255), 3);
    std::cout << "Ellipse 1: angle = " << rect1.angle << ", center = " << rect1.center << ", size = " << rect1.size << std::endl;
    std::cout << "Ellipse 2: angle = " << rect2.angle << ", center = " << rect2.center << ", size = " << rect2.size << std::endl;
    std::cout << "Ellipse 3: angle = " << rect3.angle << ", center = " << rect3.center << ", size = " << rect3.size << std::endl;
    
    cv::RotatedRect rotated3;
    rotated3 = _fit_ellipse(contours[0]);
    std::cout << "Ellipse 3: angle = " << rotated3.angle << ", center = " << rotated3.center << ", size = " << rotated3.size << std::endl;
    
    cv::RotatedRect rotated1;
    rotated1 = _fit_ellipse_direct(contours[0]);
    std::cout << "Ellipse 1: angle = " << rotated1.angle << ", center = " << rotated1.center << ", size = " << rotated1.size << std::endl;
    
    cv::RotatedRect rotated2;
    rotated2 = _fit_ellipse_ams(contours[0]);
    std::cout << "Ellipse 2: angle = " << rotated2.angle << ", center = " << rotated2.center << ", size = " << rotated2.size << std::endl;
    
    cv::imshow("src", src);
    cv::imshow("src1", src1);
    cv::imshow("src2", src2);
    cv::imshow("src3", src3);
    cv::waitKey(0);
    
    return 0;
}
